
#include "Color.h"

Color::Color(int h, float s, float v) {
  _h = h;
  _s = s;
  _v = v;
}

Color::Color(int r, int g, int b) {
  rgbToHsv(r, g, b);
}

void  Color::setHue(int h)          { _h = constrain(h, 0, 359); }
void  Color::setSaturation(float s) { _s = constrain(s, 0.0, 1.0); }
void  Color::setValue(float v)      { _v = constrain(v, 0.0, 1.0); }
int   Color::getHue()               { return _h; }
float Color::getSaturation()        { return _s; }
float Color::getValue()             { return _v; }

void Color::getRgb(byte rgb[]) {
  return hsvToRgb(_h, _s, _v, rgb);
}

void Color::rgbToHsv(int r, int g, int b){
  // RGB are from 0..1, H is from 0..360, SV from 0..1
  float rf = ((float) r) / 255.0f;
  float gf = ((float) g) / 255.0f;
  float bf = ((float) b) / 255.0f;
  
  float maxRgb = max(max(rf, gf), bf);
  float minRgb = min(min(rf, gf), bf);
  float delta = maxRgb - minRgb;
  
  float tV = maxRgb;
  float tS = 0;
  float tH = 0;
  
  if (delta == 0)
  {
  	tH = 0;
  	tS = 0;
  }
  else
  {
  	tS = delta / maxRgb;
  	float dR = 60.0f*(maxRgb - rf)/delta + 180;
  	float dG = 60.0f*(maxRgb - gf)/delta + 180;
  	float dB = 60.0f*(maxRgb - bf)/delta + 180;
  	if (rf == maxRgb)
  		tH = dB - dG;
  	else if (gf == maxRgb)
  		tH = 120 + dR - dB;
  	else
  		tH = 240 + dG - dR;
  }
  
  if (tH<0)
  	tH+=360;
  if (tH>=360)
	tH-=360;
  
  _h = tH;
  _s = tS;
  _v = tV;
  
  //Serial << "Converting RGB " << r << ", " << g << ", " << b << " to HSV " << tH << ", " << tS << ", " << tV << "\n";
}

void Color::hsvToRgb(int h, float s, float v, byte rgb[]){
  float rf, gf, bf;
  float i, f, p, q, t;
  float ht = (float) h;
  if (s == 0) 
  {
    rf = gf = bf = v;
   
  }
  else 
  { 
    ht /= 60.0f;
    i = floor(ht); // Kanskje unødvendig iom integerdiv over
    f = ht - i;
    p = v * (1.0f - s);
    q = v * (1.0f - s * ((float) f));
    t = v * (1.0f - s * (1.0f - ((float) f)));
    
    switch ((int) i) 
    {
      case 0:
        rf = v;
        gf = t;
        bf = p;
        break;
      case 1:
        rf = q;
        gf = v;
        bf = p;
        break;
      case 2:
        rf = p;
        gf = v;
        bf = t;
        break;
      case 3:
        rf = p;
        gf = q;
        bf = v;
        break;
      case 4:
        rf = t;
        gf = p;
        bf = v;
        break;
      case 5:
        rf = v;
        gf = p;
        bf = q;
        break;
    }
  }
    
  rgb[0] = (byte) (rf * 255.0f);
  rgb[1] = (byte) (gf * 255.0f);
  rgb[2] = (byte) (bf * 255.0f);
    
  //Serial << "Converting HSV " << h << ", " << s << ", " << v << " to RGB " << rgb[0] << ", " << rgb[1] << ", " << rgb[2] << "\n";
} 